;; -------------- quasi-Newton method ---------------
;; We only give BFS(Broyden, Flecher and Shanmo) method in quasi-Newton method

;; use stream pkg
(load "../../../libs-for-chezscheme/stream.ss")
(import (stream))
;; use some matrix operations
(load "../matrix-vector-operations.scm")

;; current-H => next-H
(define (next-H current-H delta-x delta-y)
  (let* ([inverse-delta-x-*-delta-y
          (/ 1.0 (vector-sum
                  (vector-map * delta-x delta-y)))]
         [Hy (matrix-*-vector current-H delta-y)]
         [mu (+ 1
                (* (vector-sum
                    (vector-map * delta-y Hy))
                   inverse-delta-x-*-delta-y))])
    (matrix-add current-H
                (matrix-scalar-product
                 inverse-delta-x-*-delta-y
                 (matrix-minus (matrix-scalar-product
                                mu
                                (cloumn-*-row delta-x delta-x))
                               (cloumn-*-row Hy delta-x)
                               (cloumn-*-row delta-x Hy))))))

;;----------- the BFS method ----------
(define (BFS-stream f x0 H0)
  (stream-cons x0
               (let* ([delta-x (vector-scalar-product
                                -1
                                (matrix-*-vector H0 (f x0)))]
                      [x1 (vector-map + x0 delta-x)]
                      [delta-y (vector-map - (f x1) (f x0))])
                 (BFS-stream f x1 (next-H H0 delta-x delta-y)))))
;; note that one needs to give H0 which is the inverse of f'(x0),
;; which needs some other method.

;;------------ BFS-accleration method -------------
;;Aitken acceleration
(define (Aitken-acceleration strm)
  (let* ([s0 (stream-ref strm 0)]
         [s1 (stream-ref strm 1)]
         [s2 (stream-ref strm 2)]
         [news (vector-map (lambda (e0 e1 e2)
                             (if (zero? (+ e0 e2 (* -2 e1)));;denominate is 0
                                 e2
                                 (- e2 (/ (expt (- e2 e1) 2)
                                          (+ e0 e2 (* -2 e1))))))
                           s0 s1 s2)])
    (stream-cons news (Aitken-acceleration (stream-cdr strm)))))
;;Super acceleration
(define (stream-of-streams trans s)
  (stream-cons s (stream-of-streams trans (trans s))))
(define (super-accleration acceler s)
  (stream-map stream-car (stream-of-streams acceler s)))
;; BFS accleration method
(define (BFS-stream-acceleration f x0 H0)
  (super-accleration Aitken-acceleration (BFS-stream f x0 H0)))



;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;test: linear systems are ok!
(let* ([x0 (vector 0 0 0)]
       [f (lambda (x)
            (vector (- (* 3 (vector-ref x 0))
                       1/2)
                    (+ (expt (vector-ref x 0) 2)
                       (* -81 (vector-ref x 1)
                          ))
                    (+ 10 (* 20 (vector-ref x 2)))))]
       [H0 (vector #(1 0 0)
                   #(0 1 0)
                   #(0 0 1))]
       [solutions (BFS-stream f x0 H0)]
       [solutions-acceleration (BFS-stream-acceleration f x0 H0)])
  (print "solution iterations x: ")
  (for-each (lambda (i)
              (print (stream-ref solutions i))
              ;;(print (f (stream-ref solutions-acceleration i)))
              ;;(print (f (stream-ref solutions i)))
              )
            (range 20))
  )


;;test: but nonlinear systems are not good!
(let* ([x0 (vector 0.1 0.1 -0.1)]
       [f (lambda (x)
            (vector (- (* 3 (vector-ref x 0))
                       (cos (* (vector-ref x 1)
                               (vector-ref x 2)))
                       1/2)
                    (+ (expt (vector-ref x 0) 2)
                       (* -81 (expt (+ (vector-ref x 1) 0.1) 2)
                          (sin (vector-ref x 2)) 1.06))
                    (+ (exp (* -1 (vector-ref x 0) (vector-ref x 1)))
                       (* 20 (vector-ref x 2))
                       (- (* 10/3 3.1415926) 1))))]
       [H0 (vector #(0.33333 0.00001 0.000016)
                   #(0.002 -0.03 0.0015)
                   #(0.0016 -0.00014 0.05))]
       [solutions (BFS-stream f x0 H0)]
       [solutions-acceleration (BFS-stream-acceleration f x0 H0)])
  (print "solution iterations x: ")
  (for-each (lambda (i)
              ;;(print (stream-ref solutions i))
              ;;(print (f (stream-ref solutions-acceleration i)))
              (print (f (stream-ref solutions i)))
              )
            (range 10))
  )


